/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
*  \class	TOccupancyGrid
*  \author 	Jari Saarinen
*  \date 	Mon Jun 12 22:16:05 2006
*  
*  This is an Occupancygrid implementation
*  The base type can be changed with the grid_t typedef
*  
*  The origin of the grid is expected to be in the middle of the grid. The frame is 
*  allways so that the positive x-axis points to right and positive y-axis to up. 
*  
*  The orgin with respect to world coordinates can be changed with @world_pos parameter
*
* 
*
*  \version: 0.1  
*/

#ifndef T_OCCUPANCY_GRID_H
#define T_OCCUPANCY_GRID_H

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <vector>
#include "dt/dt.h"   ///< Distance transform

//#define OCCUPANCY_GRID_DEBUG

#define EMPTY_PROB	0.1
#define OCC_PROB	0.9

#define PROB_VALUE_MAX (1.0)
#define	 EMPTY	(EMPTY_PROB * PROB_VALUE_MAX))
#define	 OCCU	  (OCC_PROB * PROB_VALUE_MAX))

/**
* The base type for the grid
**/
typedef float grid_t;

namespace Grid{
	struct TCoord2D{
		public:
			float x;
			float y;
			TCoord2D(){
				x = 0;
				y = 0;
			}
			TCoord2D(float _x, float _y){
				x=_x;
				y=_y;
			}
	};
	struct TPixel{
		int x;
		int y;
		TPixel(){
			x = 0;
			y = 0;
		}
		TPixel(int _x, int _y){
			x=_x;
			y=_y;
		}
		bool isValid(int Nx, int Ny){
			if(x<0 || y<0) return false;
			if(x>=Nx || y>=Ny) return false;
			return true;
		}
	};
	struct TLine{
			float x1,y1,x2,y2;
			TLine(){
				x1=0;x2=0;y1=0;y2=0;
			}
			void set(float _x1,float _y1,float _x2,float _y2){
				x1=_x1;y1=_y1;x2=_x2;y2=_y2;
			}
	};

	struct TListData{
		TCoord2D coord;
		int grid_index;

		TListData(){
			grid_index = 0;
		}
		TListData( TCoord2D _coord, int _grid_index){
			grid_index = _grid_index;
			coord = _coord;
		}
	};
	
	struct TScan{
		float *r;
		float *a;
		int N;
		TScan(){
			r=NULL;a=NULL;N=0;
		}
		TScan(float *_r,float *_a,int _N){
			set(_r,_a,_N);
		}
		~TScan(){
			freeData();
		}
		void freeData(){
			if(r) free(r);
			if(a) free(a);
			N=0;
			r=NULL;a=NULL;
		}
		void alloc(int _N){
			freeData();
			r = (float *) malloc(_N*sizeof(float));
			a = (float *) malloc(_N*sizeof(float));
			N=_N;
		}
		
		void set(float *_r,float *_a,int _N){
			freeData();
			r = (float *) malloc(_N*sizeof(float));
			a = (float *) malloc(_N*sizeof(float));
			N=_N;
			for(int i=0;i<N;i++){
				r[i] = _r[i];
				a[i] = _a[i];
			}
		}
		
	};
	/**
	 * ScanMatch pose definition
	 * 2D-pose x,y and heading 
	 */
	struct pose{
			public:
					float x; ///<[m]
					float y; ///<[m]
					float a; ///<[rad]
					pose(){
							x=0;
							y=0;
							a=0;
					}
					pose(float _x,float _y,float _a){
							x=_x;y=_y;a=_a;
					}
					void set(pose &p){
							x=p.x;
							y=p.y;
							a=p.a;
					}
					void set(float xx,float yy, float aa){
							x=xx;
							y=yy;
							a=aa;
					}
						/** 
					 * Calculates differential movement based on two global positions
					 * i.e. odometric positions 
						 */
					void setToDifferentialPose(pose odo_cur,pose odo_ref){					
							float ddx,ddy,dist,alpha;
							pose tmp;
			         ///Calculates the differential movement in odometry frame of reference
							ddx = odo_cur.x - odo_ref.x;
							ddy = odo_cur.y - odo_ref.y;
							alpha = atan2(ddy, ddx);
							dist = sqrt(ddx*ddx+ddy*ddy);
	
							tmp.x = dist * cos( alpha - odo_ref.a );
							tmp.y = dist * sin( alpha - odo_ref.a );
							tmp.a = (float)fmod((float)(odo_cur.a - odo_ref.a), (float)(2.0*M_PI)); 
							if(tmp.a < 0) tmp.a += 2*(float)M_PI;
							x = tmp.x;
							y= tmp.y;
							a = tmp.a;		
			
					}
							/**
					 * Integrates new position from this position and differential position 
					 * @param diff the differential position to be added to this position
					 * @return pose New integrated position
							 **/
					const pose integrateDifferential(const pose diff){
							pose result;
							float l,phii;
		    
							l = sqrt(diff.x*diff.x + diff.y*diff.y);
							phii = atan2(diff.y,diff.x);
				
							result.x = x + (float)cos(a+phii)*l;
							result.y = y + (float)sin(a+phii)*l;

		    // Update angle
							result.a = (float)fmod((float)(diff.a + a),(float)( 2*M_PI));
		    // fmod accepts also negative values
							if(result.a < 0) result.a += 2*(float)M_PI;
							return result; 
					}
					void to2PI(){
							a = (float)fmod((float)(a),(float)( 2*M_PI));
							if(a < 0) a += 2*(float)M_PI;
					}
					void toPI(){
							int cnt=0;
							if(a>M_PI) while(a>M_PI){ 
									a-=2.0*M_PI;
									cnt++;
									//if(cnt%10000==0) fprintf(stderr,"Loopping alot a=%.2f cnt=%d\n",a,cnt);
							}
							else if(a<-M_PI) while(a<-M_PI){ 
									a+=2.0*M_PI;
									cnt++;
									//if(cnt%10000==0) fprintf(stderr,"Loopping alot a=%.2f cnt=%d\n",a,cnt);
							}
					}
						
					~pose(){}
		
	};
}

class TOccupancyGrid{
public:
	int information_sum; ///< a varable that tells how much information is added to grid
	
	TOccupancyGrid(); ///< Default constructor
	/**
	* Constructor
	* @p size_x the size of the grid in x-direction, in meters
	* @p size_y the size of the grid in y-direction, in meters
	* @p reso   Resolution. The size of one cell in meters
	* @p origin The position of the grid origin in the world frame of reference
	**/
	TOccupancyGrid(float size_x, float size_y, float reso, Grid::TCoord2D origin);
	~TOccupancyGrid();
	//int updatePixelProbability(const TCoord2D &Pos,, float Pn);
	/**
	* Get direct access to the data pointer
	* The data is stored in one dimensional data pointer and ordered so that 
	* index (i,j)=data[j*Ny+i]
	* @return pointer to data;
	*/
	grid_t *getDataPtr();
	/**
	* Returns the dimension of grid (number of cells in x and y direction)
	* @p &x number of cells in x-axis
	* @p &y number of cells in y-axis
	*/
	void getDim(int &x, int&y);
	
	/**
	* Returns the grid position in world coordinate frame
	* @return the coordinates of the origin
	*/
	Grid::TCoord2D getWorldPos();
	
	/**
	* returns the resolution = the number of meters that one cell represents 
	*/
	float getResolution();
	
	int size(){
		return (Nx*Ny);
	}
////////////////////////////////////////////////////////////////////////////////////	
////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////	
	/**
	* Transform a world coordinate position to 
	* pixel coordinate position
	* @p P a position in world coordinates
	* @return the P in pixel coordinates
	*/
	Grid::TPixel toPixel(Grid::TCoord2D P);
	/**
	* Transform pixel coordinate to world coordinate
	* @p P a pixel position
	* @return the position of the pixel in world coordinates
	*/
	Grid::TCoord2D toCoord(Grid::TPixel P);
	/**
	 * Changes an measurement taken in pose (x,y,a) to TCoord2D
	 * @p x,y,a the robot pose in the world
	 * @p r,ra the measurement
	 **/
	Grid::TCoord2D toCoord(float x, float y, float a, float r, float ra);

	/**
	* Set value to pixel position P
	* @p P The position in grid_t
	* @p value The value that you want to set
	* @return true in success, else false
	*/
	bool setValue(Grid::TPixel P,grid_t value);
	
	/**
	* Sets value to grid in world position P
	* @p P a position in world
	* @p value the value that you want to set
	* @return true in success, else false
	*/
	bool setValue(Grid::TCoord2D P, grid_t value);
	
	/**
	* get value of the grid from world position P
	* @p P a position where you want to take a value
	* @p &value the value is copied here
	 * @return true in success, else false 
	**/
	bool getValue(Grid::TCoord2D P, grid_t &value);
	
	/**
	* Gets a value from pixel position P
	* @p P the pixel coordinate that you want to retreive the data from
	* @p &value The value is copied here
	* @return true in success, else false
	*/
	bool getValue(Grid::TPixel P, grid_t &value);
	
	
	/**
	* Returns index of given pixel
	* @p P the pixel coordinate that you want to retreive the data from
	* @p &value The value is copied here
	* @return true in success, else false
	*/
	
	bool getIndex(Grid::TPixel P, int &value);
	/**
	* Returns corresponding TPixel of a given grid index
	* @p index the grid index you want to match
	* @p &P The point is copied here
	*/
	void getPoint(int index, Grid::TPixel &P);

	/**
	* Sets the whole grid to given value
	* @p value The wanted grid value
	*/
	void setGridToValue(grid_t value);
	
	/**
	 * Updates the Observation probability @P to the grid position @Pos.
	 * @param Pos the coordinate which probability will be updated
	 * @param P The observation probability
	 * @return true
	 */
	bool updateProbability(Grid::TCoord2D Pos, float P);
	bool updateProbability(Grid::TPixel Pos, float P);
	
////////////////////////////////////////////////////////////////////////////////////	
////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////
	/**
	* Sets a line to the grid. The line is given as start and end coordinates
	* The value that the line is set is given with @value input argument
	* @p x1 x-coordinate of the start point
	* @p y1 y-coordinate of the start point
	* @p x2 x-coordinate of the end point
	* @p y2 y-coordinate of the end point
	* @p value The value that the line is set to
	 * @return true in success, else false
	 **/
	bool setLine(float x1,float y1,float x2,float y2, grid_t value);
	
	/**
	* Sets a line to grid. The line is given as start point and angle, length pair.
	* @p pos the start point of the line
	* @p r The length of the line
	* @p angle angle of the line
	* @p value The value the grid cells under the line is set to
	* @p checkRigid Set true if you have rigid structures in map
	* @p r_val the value of the rigid cell
	 * @return true in success, else false
	**/
	bool setLine(Grid::TCoord2D pos, 
							 float r, float angle, grid_t value,
							 bool checkRigid=false, grid_t r_val=0);
	/**
	 * Sets a line to grid. The line is given as start point and angle, length pair.
	 * The grid cell is set to value @value only if it was previously in value @unknown
	 * 
	 * @p pos the start point of the line
	 * @p r The length of the line
	 * @p angle angle of the line
	 * @p value The value the grid cells under the line is set to
	 * @p unknown The value that the grid need to be before it is set to @value
	 * @return true in success, else false
	 **/
		bool setLine(Grid::TCoord2D pos, 
															 float r, 
															 float angle, 
															 grid_t value, grid_t unknown);
	
	/**
	 * updates probability on a line to grid. 
	 * The line is given as start point and angle, length pair.
	 * @p pos the start point of the line
	 * @p r The length of the line
	 * @p angle angle of the line
	 * @p P The observation probability
	 * @return true in success, else false
	 **/
	bool setBayesLine(Grid::TCoord2D pos, float r, float angle, float P);
	
	
	/**
	* Sets a beam into the grid. By beam I mean a range measurement that has 
	* a limited beam width (not only a straigth line). e.g. SICK laser scanner typically has
	* beam width of 0.5degrees, the outdoor version has about 0.75degrees.  
	* @p pos the start position of the beam
	* @p r the length of the beam
	* @p angle the angle of the beam centre
	* @p beamWidth How wide is the beam
	* @p value to which value the grid is set to.
	* @return true in success, else false
	*/
	bool setBeam(Grid::TCoord2D pos, float r, float angle,float beamWidth, grid_t value);
	
	/**
	 * Set a circle to map 
	 * @p pos the position of the centre of the circle
	 * @p r The radius of the circle
	 * @p value To which value the grid is set to. 
	 * @return true in success, else false
	 */
	bool setCircle(Grid::TCoord2D pos, float r, grid_t value);
	/**
	 * Sets a an arc into the grid. The arc is defined the same way as beam, but
	 * now only the only the values at the end of @r are set (i.e arc) 
	 * @p pos the start position of the beam
	 * @p r the length of the beam
	 * @p angle the angle of the beam centre
	 * @p beamWidth How wide is the beam
	 * @p value to which value the grid is set to.
	 * @return true in success, else false
	 */
	bool setArc(Grid::TCoord2D pos, float r, float angle,float beamWidth, grid_t value);
////////////////////////////////////////////////////////////////////////////////////	
////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////
	/**
	 * This method will set the scan end points to grid 
	 * The (x,y,a) is the robot/sensor Pose in the world
	 * The s contains the scan. For each point there is a (r,a)-pair which defines the 
	 * range and angle with respect to robot of each measurement.
	 * @p x x-coordinate of the robot
	 * @p y y-coordinate of the robot
	 * @p a heading of the robot
	 * @p s The scan
	 * @p value The value the scan points are set.
	 * @return true in success, else false
	 */
	bool setScanPoints(float x, float y ,float a, Grid::TScan &s, grid_t value);
	
	/**
	 * This method will set the scan end points to grid 
	 * The grid is set to @value only if the previous value was @unknown
	 *
	 * The (x,y,a) is the robot/sensor Pose in the world
	 * The s contains the scan. For each point there is a (r,a)-pair which defines the 
	 * range and angle with respect to robot of each measurement.
	 * @p x x-coordinate of the robot
	 * @p y y-coordinate of the robot
	 * @p a heading of the robot
	 * @p s The scan
	 * @p value The value the scan points are set.
	 * @p unknown The value that the grid must be in order for the value to be set.
	 * @return true in success, else false
	 */
	 bool setScanPoints(float x, float y ,float a, Grid::TScan &s, grid_t value, grid_t unknown);
	
	/**
	 * This method will fill the scan to grid. It means that the endpoints are set as 
	 * occupied (o_val) and the points that don't contain the obstacles are set as 
	 * empty (e_val)
	 *
	 * The (x,y,a) is the robot/sensor Pose in the world
	 * The s contains the scan. For each point there is a (r,a)-pair which defines the 
	 * range and angle with respect to robot of each measurement.
	 * @p x x-coordinate of the robot
	 * @p y y-coordinate of the robot
	 * @p a heading of the robot
	 * @p s The scan
	 * @p e_val The value the scan hit points are set = occupied
	 * @p o_val The value for empty cell
	 * @return true in success, else false
	 */
	bool setScanFillSimple(float x, 
												 float y ,
												 float a, 
												 Grid::TScan &s, 
												 grid_t e_val,
												 grid_t o_val);
	/**
	 * This method will fill the scan to grid. It means that the endpoints are set as 
	 * occupied (o_val) and the points that don't contain the obstacles are set as 
	 * empty (e_val)
	 * 
	 * Additionally this method checks if the cell is in the value @unknown. If not, 
	 * it is not updated.  
	 *
	 * The (x,y,a) is the robot/sensor Pose in the world
	 * The s contains the scan. For each point there is a (r,a)-pair which defines the 
	 * range and angle with respect to robot of each measurement.
	 * @p x x-coordinate of the robot
	 * @p y y-coordinate of the robot
	 * @p a heading of the robot
	 * @p s The scan
	 * @p e_val The value the scan hit points are set = occupied
	 * @p o_val The value for empty cell
	* @p u_val Value of unknown cell
	 * @return true in success, else false
	 */
	bool setScanFillSimple(float x, 
												 float y ,
												 float a, 
												 Grid::TScan &s, 
												 grid_t e_val,
												 grid_t o_val,
												 grid_t u_val);
	
	/**
	* This method is the same as setScanFillSimple(), but instead of simply
	* setting the grid cells into some value, the cell probabilities are updated
	* with bayes formula. 
	* This method uses define PROB_VALUE_MAX. This value can be used if 
	* you have e.g. grid_t set to char (use PROB_VALUE_MAX 255)
	 * @p x x-coordinate of the robot
	 * @p y y-coordinate of the robot
	 * @p a heading of the robot
	 * @p s The scan
	 * @p Pe empty measurement probability
	 * @p Po occupied probability 
	 * @return true in success, else false
	**/
	bool setScanFillProb(float x, float y ,float a, Grid::TScan &s,float Pe,float Po);
	/**
	 * This is the same method as setScanFillSimple(), but this method will not changed
	 * the rigid structures in the map i.e. if the cell is in the r_val, then it will not beam
	 * updated, no matter what the measurement says.
	 *
	 * The (x,y,a) is the robot/sensor Pose in the world
	 * The s contains the scan. For each point there is a (r,a)-pair which defines the 
	 * range and angle with respect to robot of each measurement.
	 * @p x x-coordinate of the robot
	 * @p y y-coordinate of the robot
	 * @p a heading of the robot
	 * @p s The scan
	 * @p e_val The value the scan hit points are set = occupied
	 * @p o_val The value for empty cell
	 * @p r_val The value for the rigid obstacle
	 * @return true in success, else false
	 */
	bool setScanFillWithRigidWalls(float x, 
																 float y ,
																 float a, 
																 Grid::TScan &s, 
																 grid_t e_val,
																 grid_t o_val,
																 grid_t r_val);
	////////////////////////////////////////////////////////////////////////////////////	
	////////////////////////////////////////////////////////////////////////////////////
	////////////////////////////////////////////////////////////////////////////////////
	/**
	* Copies the grid data to RGB pointer
	* The value of cell is divided with @scale
	* The method fills the RGB as grey scale image 
	* The rgb pointer must be reserved. Size is expected to be 3*Nx*Ny 
	* @p *rgb pointer to RGB data
	* @p scale divider for the grid data
	* @return true
	*/
	bool fillRGB(unsigned char *rgb, float scale);
	/**
	* Copies the grid data to RGB pointer
	* The method uses empty, occupied and rigid values to 
	* set R=rigid,G=free and B=occupied values
	* If the value is none of the above, it is set to black
	* The rgb pointer must be reserved. Size is expected to be 3*Nx*Ny
	*
	* @p *rgb The RGB data pointer
	* @p e_val Value for empty=free cells
	* @p o_val Value for occupied cells
	* @p r_val Value for Rigid cells
	* @return true
	*/
	bool fillRGB(unsigned char *rgb, grid_t e_val, grid_t o_val, grid_t r_val);		
	
////////////////////////////////////////////////////////////////////////////////////	
////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////
	
	/**
	* Returns a line taken from position (x,y) to direction a
	* The line ends when cell value in direction a is equal or grater than
	* o_val. Method returns the metric distance.
	* @p x world coordinate x
	* @p y world coordinate y
	* @p a direction of line
	* @return the range. -1 is returned if no hit.
	**/
	float getLine(float x,float y, float a,grid_t o_val);
	
	/**
	 * Returns a line taken from position (x,y) to direction a
	 * The line ends when cell value in direction a is equal or grater than
	 * o_val. Method returns the metric distance.
	 * @p x world coordinate x
	 * @p y world coordinate y
	 * @p a direction of line
	 * @p o_val threshold
	 * @p *dt disance transformation matrix 
	 * @return the range. -1 is returned if no hit.
	 **/
	float getLine(float x,float y, float a,grid_t o_val, float *dt);
	
	/**
	* returns a scan taken from (x,y). The first measurement is taken from angle @a 
	* @n is the number of measurements taken and @da is the angle difference between
	* measurements. The result is stored to @s. If the s.N != n then it is reallocated.
	* The o_val is used to check if the cell has obstacle
	* @p x world coordinate x
	* @p y world coordinate y
	* @p a direction of the first measurement
	* @p n number of measurements taken
	* @p da the angle difference between the measurements 
	* @p o_val The threshold for checking if the cell has obstacle
	* @p s The scan s.r[] will be -1 if no measurement can be taken 
	* @return true
	**/
	bool getScan(float x,float y, float a, int n, float da, grid_t o_val, Grid::TScan &s);
	/**
	 * This is the same method as above one, but it uses a distance transformation
	 * matrix to make the calculation more efficient
	 * returns a scan taken from (x,y). The first measurement is taken from angle @a 
	 * @n is the number of measurements taken and @da is the angle difference between
	 * measurements. The result is stored to @s. If the s.N != n then it is reallocated.
	 * The o_val is used to check if the cell has obstacle
	 * 
	 * @p x world coordinate x
	 * @p y world coordinate y
	 * @p a direction of the first measurement
	 * @p n number of measurements taken
	 * @p da the angle difference between the measurements 
	 * @p o_val The threshold for checking if the cell has obstacle
	 * @p *dt distance transform matrix calculated with distanceTransform()
 	 * @p s The scan s.r[] will be -1 if no measurement can be taken 
	 * @return true
	 **/
	bool getScan(float x,
							 float y, 
							 float a, 
							 int n, 
							 float da, 
							 grid_t o_val,
							 float *dt, 
							 Grid::TScan &s);
	
		/**
	 * Get submap (copy) from the grid.
	 * The centre of the submap is @p centre. The left upper corner is centre.x-Nx/2,centre.y-Ny/2
	 * @param *submap The submap is copied here. Must be reserved size (nx*ny)
	 * @param centre The "center" pixel around which the submap is taken from
	 * @param nx size of submap in x-direction
	 * @param ny size of the submap in y-dir.
	 */
		bool getSubmap(float *submap, Grid::TPixel centre, int nx, int ny);
		
		/**
		 * Set submap (copy) to the grid.
		 * The centre of the submap is @p centre. The left upper corner is centre.x-Nx/2,centre.y-Ny/2
		 * @param *submap The submap is copied here. Must be reserved size (nx*ny)
		 * @param centre The "center" pixel around which the submap is taken from
		 * @param nx size of submap in x-direction
		 * @param ny size of the submap in y-dir.
		 */
		bool setSubmap(float *submap, Grid::TPixel centre, int nx, int ny);
	
	/**
	* Makes a distance transform for the grid. The algorithm uses binary image, so 
	* th is used to define if the cell is occupied or not
	* @p *out Nx*Ny sized pointer that the distances are stored into 
	* @p th Threshold for segmentation
	* @return maximum distance
	**/
	float distanceTransform(float *out, grid_t th);	 
	
	/**
	 * Returns the minimum of the surroundig of pixel in pos (x,y)
	 * @p x x index in grid
	 * @p y y index in grid
	 * @return the minimum value around (x,y)
	 */
	grid_t getMin(int x, int y);
	
	/**
	* Computes a gradient of the grid. 
	* The method computes the gradient as [value-(minimum of neighbour cells)]/resolution 
	* @param *out Output pointer. If null the memory is reserved
	* @return maximum grid value
	**/
	float gradient(grid_t *out);
	
////////////////////////////////////////////////////////////////////////////////////	
////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////
	/**
	* Saves the data to file
	* Header = first line:
	* fprintf(f,"%d %d %f %f %f\n",Nx,Ny,resolution,world_pos.x,world_pos.y);
	* @p *fname Name of the file to save to.
	* @return true, if success, else false
 	*/
	bool save(const char *fname);
	
	/**
	* Loads the data from the file 
	*/
	bool load(const char *fname);
	
	/**
	* Loads the data in Ground Model format
	**/
	bool loadGModel(const char *fname);
	
	/**
	* Loads a line map (in format '%f %f %f %f\n')
	* and puts it to appropriate sized map
	* @p *fname the filename
	* @p scale  the scale for the line map (the number from the file is multiplied with this)
	* @p lineval The value that the line is set to
	* @p rescale set false if you don't want that the map is rescaled when the map is read.
	* @p reset Set true if you want to reset the grid to value @reset_val
	* @p reset val the value to reset the grid to
	* @return true if success.
	**/
	bool loadLineMap(const char *fname, float scale,grid_t lineval,bool rescale=true,bool reset=true,grid_t reset_val=0.5);
	

	
	private:
		grid_t *data;
		int Nx;
		int Ny;
		float resolution;
		Grid::TCoord2D world_pos;
		
		 
		void initDefaults(){
			data = NULL;
			Nx = 100;
			Ny = 100;
			resolution = 0.1;
			information_sum = 0;
		}
		void mallocData(){
			if(data) free(data);
			data = (grid_t *) malloc(Nx*Ny*sizeof(grid_t));
		}
		
		bool isValid(Grid::TPixel P){
			return(P.x>=0 && P.x<Nx && P.y>=0 && P.y<Ny);
		}
		
};          


#endif


